#include "stdafx.h"

#include "LocatorPoseMex.h"

void runEllipseFitAlgebraic(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
	if (nrhs != 2) mexErrMsgTxt("EllipseFitAlgebraic needs 1 argument.");
	
	arma::mat Y = convertMatlabToArma(prhs[1]);			// points
	
	double alpha = 0;
	double a = 0;
	double b = 0;
	arma::colvec center = arma::zeros<arma::colvec>(2);
	arma::colvec s_center = arma::zeros<arma::colvec>(2);
	
	try {
		OTPF::Fit::Ellipse::fitAlgebraic(Y, alpha, a, b, center, s_center);
	} catch(OTPF::Error::Matrix_size e) {
		mexErrMsgTxt(e.p);
	} catch(...) {
		mexErrMsgTxt("Unknown error in OTPF::Ellipse::fitAlgebraic.");
	}
	
	plhs[0] = mxCreateDoubleMatrix(1, 1, mxREAL);		// alpha
	memcpy(mxGetPr(plhs[0]), &alpha, sizeof(double));
	plhs[1] = mxCreateDoubleMatrix(1, 1, mxREAL);		// half axis a	
	memcpy(mxGetPr(plhs[1]), &a, sizeof(double));
	plhs[2] = mxCreateDoubleMatrix(1, 1, mxREAL);		// half axis b
	memcpy(mxGetPr(plhs[2]), &b, sizeof(double));
	plhs[3] = convertArmaToMatlab(center);				// center
	plhs[4] = convertArmaToMatlab(s_center);			// standard deviation center
	
	
	//freeArma(Y, pY_orig);
	return;
};
